We want to be able to rotate and move at once using swerve, right now we are limited to turning around the center, or moving around.
I read the solution that was on the linked PDF. It doesn't do a very good job explaining, while it works, I don't like using a "black box," especially since this is supposed to be more educational than "whip up something fast that works." Unable to explain the magic the algorithm uses, I set out making my own.
We can break the problem into two separate translations and use the magic of adding vectors to find the resultant vector.
First is a rotation. A wheel is at angle x to the front of the robot and is r distance away. If we rotate the robot y degrees, relative to the initial position of the robot, the wheel is x+y degrees from the front, and still r away.
Initial point: (r * cosx, r * sinx)
after rotation: (r * cos(x+y), r * sin(x+y))
We get the vector by subtracting the first point be the second one
rotation: <(r * cos(x+y)) - (r * cosx), (r *sin(x+y)) - (r * sinx)>
Then we have the translation. We just add the x and the y of the translation vector for this step. Turning a degree and magnitude into x and y.
x = TranslationSpeed * cosTranslationAngle
y = TranslationSpeed * sinTranslationAngle
We then simply add the 2 vectors together to find the vector at which we should have the wheel. Turn this vector into a degree via magic of arctan, and use pythatgorean for speed to run the motor.
We want to be able to rotate and move at once using swerve, right now we are limited to turning around the center, or moving around.
I read the solution that was on the linked PDF. It doesn't do a very good job explaining, while it works, I don't like using a "black box," especially since this is supposed to be more educational than "whip up something fast that works." Unable to explain the magic the algorithm uses, I set out making my own.
We can break the problem into two separate translations and use the magic of adding vectors to find the resultant vector.
First is a rotation. A wheel is at angle x to the front of the robot and is r distance away. If we rotate the robot y degrees, relative to the initial position of the robot, the wheel is x+y degrees from the front, and still r away.
Initial point: (r * cosx, r * sinx)
after rotation: (r * cos(x+y), r * sin(x+y))
We get the vector by subtracting the first point be the second one
rotation: <(r * cos(x+y)) - (r * cosx), (r *sin(x+y)) - (r * sinx)>
Then we have the translation. We just add the x and the y of the translation vector for this step. Turning a degree and magnitude into x and y.
x = TranslationSpeed * cosTranslationAngle
y = TranslationSpeed * sinTranslationAngle
We then simply add the 2 vectors together to find the vector at which we should have the wheel. Turn this vector into a degree via magic of arctan, and use pythatgorean for speed to run the motor.